`timescale 1ns / 1ps

module centure_move(
    input   wire             sys_clk     ,
    input   wire             reset_n     ,
    input   wire             en_flag     , // 开始抓取
    input   wire             begin_reg   , // 分步开始
    // 输入信号 
    input   wire     [7:0]   in_x        , // 抓取的x
    input   wire     [7:0]   in_y        , // 抓取的y

    input   wire     [7:0]   to_x        , // 松开的x
    input   wire     [7:0]   to_y        , // 松开的y

    input   wire     [11:0]  move_time   , // 移动时间（可以不需要）

    output  reg              dir         , // 抓取方向
    output  reg      [11:0]  claws       , // 抓取力度
    output  reg      [7:0]   out_x       ,
    output  reg      [7:0]   out_y       ,
    output  reg              out_z       , // 1 下抓 0 抬起
    output  reg              real_en     , // 是否需要校准
    output  reg      [11:0]  out_time    , // 实际使用时间

    output  reg              camera_updata ,
    output  reg              begin_flag  , // 分步启动信号
    output  reg              finish_flag 
);

// 初始化位置
parameter   basic_x =   8'd125  ;
parameter   basic_y =   8'd140  ;
parameter   basic_t =   12'd1500; // 正中间
parameter   basic_c =   12'd2500; // 强力吸

// 状态机变量
parameter S0 = 12'b0000_0000_0001;// 转到指定位置
parameter S1 = 12'b0000_0000_0010;// 向下伸机械臂并吸取
parameter S2 = 12'b0000_0000_0100;// 抬起来
parameter S3 = 12'b0000_0000_1000;// 转动到目标位置
parameter S4 = 12'b0000_0001_0000;// 过渡1
parameter S5 = 12'b0000_0010_0000;// 过渡2
parameter S6 = 12'b0000_0100_0000;// 向下伸机械臂
parameter S7 = 12'b0000_1000_0000;// 松开
parameter S8 = 12'b0001_0000_0000;// 抬起(防止打到别的)
parameter S9 = 12'b0010_0000_0000;// 过渡1
parameter S10= 12'b0100_0000_0000;// 过渡2
parameter S11= 12'b1000_0000_0000;// 结束
/* 主控 */
reg     [11:0]   state_rag;
reg              is_going ;
wire             centure_move_flag;

assign centure_move_flag = is_going ? begin_reg : en_flag;

// 未开始时,只受到 en_flag 影响，其他情况只受到 begin_reg 影响
always@(posedge sys_clk or negedge reset_n) begin
    if(!reset_n) begin //初始化
        is_going <= 1'b0;
    end else if(en_flag) begin
        is_going <= 1'b1;
    end else if(finish_flag) begin
        is_going <= 1'b0;
    end else begin
        is_going <= is_going;
    end
end

// 一段式状态机
always@(posedge sys_clk or negedge reset_n) begin
    if(!reset_n) begin //初始化
        state_rag   <= S0          ;
        finish_flag <= 1'b0        ;
        begin_flag  <= 1'b0        ;
        dir         <= 1'b0        ;
        claws       <= basic_t     ;
        out_x       <= basic_x     ;
        out_y       <= basic_y     ;
        out_z       <= 1'b0        ;
        real_en     <= 1'b0        ;
        out_time    <= (basic_t >> 1); // 确定的值
        camera_updata <= 1'b0      ;
    end else if(centure_move_flag) begin // 总体开始或者是分步开始
        case(state_rag)
            S0:begin // 转到指定位置
                out_x   <= in_x;
                out_y   <= in_y;
                out_z   <= 1'b0; // 抬起状态
                real_en <= 1'b1; // 需要校准
                out_time<= move_time;
                dir     <= 1'b0;

                begin_flag <= 1'b1;
                state_rag  <= S1;
            end
            S1:begin // 向下伸机械臂并吸取
                out_z   <= 1'b1; // 放下状态
                real_en <= 1'b0; // 不需要校准
                out_time<= 12'd225; // 下冲
                claws   <= basic_c; // 吸
                
                begin_flag <= 1'b1;
                state_rag  <= S2;
            end
            S2:begin // 抬起来
                out_z <= 1'b0;
                out_time<= move_time;

                begin_flag <= 1'b1;
                state_rag  <= S3;
            end
            S3:begin // 过渡状态 1
                out_x <= -basic_x;
                out_y <= basic_y;

                begin_flag <= 1'b1;
                state_rag  <= S4;
            end
            S4:begin // 过渡状态 2
                out_x <= basic_x; // 镜像
                dir   <= 1'b1   ; // 换方向
                out_time <= (move_time << 1); // 大摇摆

                begin_flag <= 1'b1;
                state_rag  <= S5;
            end
            S5:begin // 转动到目标位置
                out_x <= to_x;
                out_y <= to_y;
                out_time <= move_time;
                real_en <= 1'b1; // 需要校准

                camera_updata <= 1'b1; // 摄像头开始采集数据
                begin_flag <= 1'b1;
                state_rag  <= S6;
            end
            S6:begin // 向下伸机械臂
                out_z <= 1'b1;
                real_en <= 1'b0; // 不需要校准
                out_time <= move_time;

                begin_flag <= 1'b1;
                state_rag <= S7;
            end
            S7:begin // 松开并抬起(防止打到别的)
                out_z <= 0;
                claws <= basic_t;
                
                begin_flag <= 1'b1;
                state_rag <= S8;
            end
            S8:begin // 过渡状态 1
                out_x <= basic_x;
                out_y <= basic_y;

                begin_flag <= 1'b1;
                state_rag  <= S9;
            end
            S9:begin // 过渡状态 2
                out_x <= -basic_x;
                dir   <= 1'b0   ; // 换方向
                out_time <= (move_time << 1); // 大摇摆

                begin_flag <= 1'b1;
                state_rag  <= S10;
            end
            S10:begin 
                finish_flag <= 1'b1;
                state_rag <= S0;
            end
        endcase
    end else begin
        finish_flag <= 1'b0        ;
        begin_flag  <= 1'b0        ;
        camera_updata <= 1'b0      ;
        state_rag   <= state_rag   ;
        dir         <= dir         ;
        claws       <= claws       ;
        out_x       <= out_x       ;
        out_y       <= out_y       ;
        out_z       <= out_z       ;
        real_en     <= real_en     ;
        out_time    <= out_time    ;
    end
end

endmodule
